#ifndef HOFA_HPP
#define HOFA_HPP

#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/algorithm/model.hpp>
#include <pinocchio/algorithm/crba.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/algorithm/joint-configuration.hpp>
#include <Eigen/Dense>
#include <iostream>
#include <pinocchio/spatial/fwd.hpp>
#include <pinocchio/multibody/fwd.hpp>
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/multibody/data.hpp>
#include <ros/ros.h>
#include <ros/package.h>

class CoefficientMatrix {
  public:
    CoefficientMatrix(const std::string& urdf_filename);
    void print_model_info() const;
    void compute_inertia_matrix(const Eigen::VectorXd& q);
    void compute_coriolis_matrix(const Eigen::VectorXd& q, const Eigen::VectorXd& v);
    void compute_gravity_vector(const Eigen::VectorXd& q);
    void verify_inertia_matrix_symmetry() const;

    pinocchio::Model model;
    pinocchio::Data data;
    Eigen::MatrixXd M;
    Eigen::MatrixXd C;
    Eigen::VectorXd G;
};

class NonlinearDisturbanceObserver {
  public:
    NonlinearDisturbanceObserver(const Eigen::MatrixXd& mp);

    // 计算 z 的数值解（欧拉法）, 计算 d 的估计值
    void estimate_d(const Eigen::MatrixXd& M,
                    const Eigen::MatrixXd& C,
                    const Eigen::MatrixXd& G,
                    const Eigen::VectorXd& ddq,
                    const Eigen::VectorXd& u,
                    const double& dt);

    void simple_d(const Eigen::MatrixXd& M,
                  const Eigen::MatrixXd& C,
                  const Eigen::MatrixXd& G,
                  const Eigen::VectorXd& dq,
                  const Eigen::VectorXd& ddq,
                  const Eigen::VectorXd& u,
                  const double& dt);
    Eigen::MatrixXd m_p;         // 30x30 矩阵
    Eigen::VectorXd z;           // 30x1 向量
    Eigen::VectorXd d_estimated; // 30x1 向量
    Eigen::VectorXd d_simple;    // 30x1 向量
};

class HOFA {
  public:
    HOFA(const int& n);


    void construct_controller(const Eigen::MatrixXd& F,
                              const Eigen::MatrixXd& Z,
                              const Eigen::MatrixXd& M,
                              const Eigen::MatrixXd& C,
                              const Eigen::MatrixXd& G,
                              const Eigen::VectorXd& qe,
                              const Eigen::VectorXd& dqe,
                              const Eigen::VectorXd& d_estimated);
    int sys_dimension;
    Eigen::VectorXd uc;
    Eigen::VectorXd uf;
    Eigen::VectorXd u;
};

#endif // HOFA_HPP